#include <iostream>
#include <cerrno>
#include <arpa/inet.h>
#include "BlueToothServer.h"

using namespace Marco;
using namespace BlueTooth;

Service::Service(const std::string &serviceUUID,
                 const std::string &serviceName,
                 const std::string &serviceProvider,
                 const std::string &serviceDescription,
                 uint8_t rfcommChannel) :
   mL2CapList(NULL), mRFCommList(NULL), mRootList(NULL),
   mProtoList(NULL), mAccessProtoList(NULL), mClassIDList(NULL),
   mChannel(NULL), mPSM(NULL), mSDPRecord(NULL), mSDPSession(NULL)
{
   mUUID = serviceUUID;
   mRFChannel = rfcommChannel;

   mSDPRecord = sdp_record_alloc();

   // convert UUID string to bytes
   bdaddr_t uuidInBa;
   str2ba(mUUID.c_str(), &uuidInBa);

   uint8_t service_uuid_int[] = {0x07,0x29,0x3d,0xb4,0xa3, 0x23, 0x4e, 0x07,0x8b, 0x8b,0x25,0x0b,0x34,0x0e, 0x42, 0xa4};
   //uint8_t service_uuid_int[] = {0x00,0x00,0x00,0x00,0x00, 0x00, 0x00, 0x00,0x00, 0x00,0x00,0x00,0x00,0x00, 0x00, 0x00};

   uint8_t *x = service_uuid_int;
   printf("%02x, %02x, %02x, %02x, %02x, %02x, %02x, %02x\n",// %02x, %02x, %02x, %02x, %02x, %02x, %02x, %02x\n",
          x[0], x[2], x[4], x[6], x[8], x[10], x[12], x[14]);

   // Set the general service ID
   //sdp_uuid128_create(&mServiceUUID, &uuidInBa);
   sdp_uuid128_create(&mServiceUUID, &service_uuid_int);
   sdp_set_service_id(mSDPRecord, mServiceUUID);

   // make the service record publicly browsable
   sdp_uuid16_create(&mRootUUID, PUBLIC_BROWSE_GROUP);
   mRootList = sdp_list_append(0, &mRootUUID);
   sdp_set_browse_groups(mSDPRecord, mRootList);

   // Set L2CAP information
   sdp_uuid16_create(&mL2CapUUID, L2CAP_UUID);
   mL2CapList = sdp_list_append(0, &mL2CapUUID);
   mProtoList = sdp_list_append(0, mL2CapList);

   // Set the RFCOMM information
   sdp_uuid16_create(&mRFCommUUID, RFCOMM_UUID);
   mChannel = sdp_data_alloc(SDP_UINT8, &mRFChannel);
   mRFCommList = sdp_list_append(0, &mRFCommUUID);
   sdp_list_append(mRFCommList, mChannel);
   sdp_list_append(mProtoList, mRFCommList);

   // Attach the protocol information to the service record
   mAccessProtoList = sdp_list_append(0, mProtoList);
   sdp_set_access_protos(mSDPRecord, mAccessProtoList);

   // Set the service classes
   mClassIDList = sdp_list_append(0, &mServiceUUID);
   sdp_set_service_classes(mSDPRecord, mClassIDList);

   // Set the service name, provider and description
   sdp_set_info_attr(mSDPRecord, serviceName.c_str(),
                              serviceProvider.c_str(),
                              serviceDescription.c_str());

   // Connect to the SDP server, register the service record
   // and disconnect.
   mSDPSession = sdp_connect(BDADDR_ANY, BDADDR_LOCAL, SDP_RETRY_IF_BUSY);
   int status = sdp_record_register(mSDPSession, mSDPRecord, 0);

   // Clean the lists before the exception in case the registration failed.
   sdp_data_free(mChannel);
   sdp_list_free(mL2CapList, 0);
   sdp_list_free(mRFCommList, 0);
   sdp_list_free(mRootList, 0);
   sdp_list_free(mAccessProtoList, 0);

   if(status != 0)
   {
      std::string ex = "Service::Service() Failed to register service with SDP server with error:\n";
      char errorMsg[128];
      ex += strerror_r(errno, errorMsg, 128);
      throw(ex);
   }
}

Service::~Service()
{
   sdp_close(mSDPSession);
}

ClientSocket::ClientSocket(int socket)
{
   if(socket < 0)
   {
      throw("ClientSocket::ClientSocket() Invalid socket id, exiting.");
   }
   mSocket = socket;
}

ClientSocket::~ClientSocket()
{
   close(mSocket);
}

Event ClientSocket::Read()
{
//   while(1)
   {
      Event event;
      char msg[17] = { 0 };
      //int count = read(mSocket, &event, sizeof(Event));
      int count = read(mSocket, msg, sizeof(msg));
      std::cout << "Server::ClientSocket() received: " << count << " bytes, size of Event = " << sizeof(msg) << std::endl;
      std::cout << sizeof(char) << ", " << sizeof(int) << ", " << sizeof(float) << std::endl;

      event.Source = msg[0];
      int i;
      memcpy(&i, &msg[1], 4);
      event.KeyCode = ntohs(i);
      memcpy(&i, &msg[5], 4);
      event.KeyEvent = ntohs(i);

      float f;
      memcpy(&f, &msg[9], 4);
      event.Dx = ntohf(f);
      memcpy(&f, &msg[13], 4);
      event.Dy = ntohf(f);
      return(event);
   }
//   close(mSocket);
}

float ClientSocket::ntohf( const float inFloat )
{
   float retVal;
   char *floatToConvert = ( char* ) & inFloat;
   char *returnFloat = ( char* ) & retVal;

   // swap the bytes into a temporary buffer
   returnFloat[0] = floatToConvert[3];
   returnFloat[1] = floatToConvert[2];
   returnFloat[2] = floatToConvert[1];
   returnFloat[3] = floatToConvert[0];

   return retVal;
}

Server::Server(const std::string &serviceUUID,
               const std::string &serviceName,
               const std::string &serviceProvider,
               const std::string &serviceDescription,
               uint8_t rfCommChannel)
{
   bzero(&mBuff, sizeof(mBuff));
   // Register the requested service with the SDP
   try
   {
      mSDPService = new Service(serviceUUID, serviceName,
                                serviceProvider, serviceDescription,
                                rfCommChannel);
   }
   catch(const std::string &ex)
   {
      throw("BlueTooth::Server::Server()\n" + ex);
   }

   // Wait in a loop until a connection is received
   while(1)
   {
      mClient = -1;
      // Allocate a socket
      mSocket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
      mOptions = sizeof(mRemoteAddress);

      // Bind the socket to the requested channel on the adapter
      mLocalAddress.rc_family = AF_BLUETOOTH;
      mLocalAddress.rc_bdaddr = *BDADDR_ANY;
      mLocalAddress.rc_channel = rfCommChannel;
      bind(mSocket, (struct sockaddr *)&mLocalAddress, sizeof(mLocalAddress));

      // Put the socket into listening mode
      std::cout << "Server(): Listening for connections..." << std::endl;
      listen(mSocket, 1);

      // Accept one connection
      std::cout << "Server(): Accepting... " << std::endl;
      mClient = accept(mSocket, (struct sockaddr *)&mRemoteAddress, &mOptions);
      ba2str(&mRemoteAddress.rc_bdaddr, mBuff);
      std::cout << "Server(): Accepted a client connection from: " << mBuff << std::endl;

      bzero(&mBuff, sizeof(mBuff));
      break;
   }

}

Server::~Server()
{
   delete mSDPService;
   close(mSocket);
}
/*
int Scan()
{
    inquiry_info *ii = NULL;
    int max_rsp, num_rsp;
    int dev_id, sock, len, flags;
    int i;
    char addr[19] = { 0 };
    char name[248] = { 0 };

    dev_id = hci_get_route(NULL);
    sock = hci_open_dev( dev_id );
    if (dev_id < 0 || sock < 0) {
        perror("opening socket");
        return(1);
    }

    len  = 8;
    max_rsp = 255;
    flags = IREQ_CACHE_FLUSH;
    //ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
    ii = new inquiry_info[max_rsp];
    
    num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
    if( num_rsp < 0 ) perror("hci_inquiry");

    std::cout << "NumRsp: " << num_rsp << std::endl;

    for (i = 0; i < num_rsp; i++) {
        ba2str(&(ii+i)->bdaddr, addr);
        memset(name, 0, sizeof(name));
        if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), 
            name, 0) < 0)
        strcpy(name, "[unknown]");
        //printf("%s  %s\n", addr, name);
        std::cout << addr << ", " << name << std::endl;
    }

   std::cout << "Done." << std::endl;
    //free( ii );
    delete [] ii;
    close( sock );
    return 0;
}

int ServerThread(void *arg)
{
   struct sockaddr_rc loc_addr = { 0 }, rem_addr = { 0 };
    char buf[1024] = { 0 };
    int s, client, bytes_read;
    socklen_t opt = sizeof(rem_addr);

    // allocate socket
    s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);

    // bind socket to port 1 of the first available 
    // local bluetooth adapter
    loc_addr.rc_family = AF_BLUETOOTH;
    loc_addr.rc_bdaddr = *BDADDR_ANY;
    loc_addr.rc_channel = (uint8_t) 0;
    bind(s, (struct sockaddr *)&loc_addr, sizeof(loc_addr));

    // put socket into listening mode
    listen(s, 1);

    // accept one connection
    client = accept(s, (struct sockaddr *)&rem_addr, &opt);

    ba2str( &rem_addr.rc_bdaddr, buf );
    fprintf(stderr, "accepted connection from %s\n", buf);
    memset(buf, 0, sizeof(buf));

    // read data from the client
    bytes_read = read(client, buf, sizeof(buf));
    if( bytes_read > 0 ) {
        printf("received [%s]\n", buf);
    }

    // close connection
    close(client);
    close(s);
    return 0
}

int main(int argc, char **argv)
{
}*/
